clear all;
close all;


m=1.12;g=9.81;
Ix=0.0119;Iy=0.0119;Iz=0.0223;
b=0.00000773213;d=0.000000127513;Jr=0.00085;l=0.23;
kp1=1.2;kp2=1.2;kp3=1.2;kp4=5.62;
kint1=3.1;kint2=3.1;kint3=3.1;kint4=7.2;
kd1=0.08;kd2=0.08;kd3=0.03;kd4=1.0;
beta1=1.0;beta2=1.0;beta3=3;beta4=2.0;
lamb1=1.2;lamb2=1.2;lamb3=4;lamb4=1.01;
gama1=0.72;gama2=0.72;gama3=0.21;gama4=22;
OHM=1000;

Ixx=0.0119;Iyy=0.0119;Izz=0.0223;
a1=(Iyy-Izz)/Ixx;a2=Jr/Ixx;
a3=(Izz-Ixx)/Iyy;a4=-Jr/Iyy;
a5=(Ixx-Iyy)/Izz;b1=l/Ixx;
b2=l/Iyy;b3=l/Izz;

kr1=4.6;kr2=4.6;kr3=7.4;
ko1=0.86;ko2=0.86;ko3=0.86;
k2=14.3;fi1=1.5;J=0.01;
kf1=0.05;kf2=0.05;kf3=0.1;k3=0.1;k2=1.43;

Qk=[1;0;1;0;0.5;0;0;0;0;0;0;0];% ode45不能直接用矩阵，得变成向量进行操作；依次是phi,th,fi,h
% Zk=[0;0;0;0;0;0;0;0;0.72;0.72;0.21;22];
Zk=[0;0;0;0];
% v=[0;0;0;0];
% Zk=[0;0;0];

T=0.001;
for k=1:1:30000
  
    time(k)=k*T;

    phid(k)=0;
    thd(k)=0;
    fid(k)=0;
    hd(k)=5;
    
    Qd(:,k)=[phid(k);thd(k);fid(k);hd(k)];             
     
%     if(k*T>=5)
%         dt(1)=2*cos(2*pi/3*k*T);
%         dt(2)=7+2*cos(2*pi/3*k*T);
%         dt(3)=5+2*cos(pi/2*k*T);
%         dt(4)=18+4*cos(pi/6*k*T);
%     else
        dt(1)=0;dt(2)=0;dt(3)=0;dt(4)=0;
%     end

    phi(k)=Qk(1);dphi(k)=Qk(2);th(k)=Qk(3);dth(k)=Qk(4);
    fi(k)=Qk(5);dfi(k)=Qk(6);h(k)=Qk(7);dh(k)=Qk(8);
    x(k)=Qk(9);dx(k)=Qk(10);y(k)=Qk(11);dy(k)=Qk(12);

    x1=Qk(1);x2=Qk(2);x3=Qk(3);x4=Qk(4);
    x5=Qk(5);x6=Qk(6);x7=Qk(7);x8=Qk(8);
    x9=Qk(9);x10=Qk(10);x11=Qk(11);x12=Qk(12);
    
    oe1=Zk(1);oe2=Zk(2);oe3=Zk(3);oef=Zk(4);
%     gama1=Zk(9);gama2=Zk(10);gama3=Zk(11);gama4=Zk(12);
    
    Q(:,k)=[phi(k);th(k);fi(k);h(k)];
    dQ(:,k)=[dphi(k);dth(k);dfi(k);dh(k)];
    
    
    e(:,k) = Q(:,k)-Qd(:,k);
    e1=e(1,k);e2=e(2,k);e3=e(3,k);e4=e(4,k);
    de(:,k) = dQ(:,k);
    de1=de(1,k);de2=de(2,k);de3=de(3,k); de4=de(4,k); 

    do1(k) = oe1 + kf1*J*de1;
    do2(k) = oe2 + kf2*J*de2;
    do3(k) = oe3 + kf3*J*de3;
    dp(k) = oef + k3*m*de4;
    ot(:,k)=[do1(k);do2(k);do3(k);dp(k)];

    vt1(k)=(g-k2*de4-fi1*e4-dt(4))*m/(cos(x1)*cos(x3));
    vt2(k)=(-(x4*x6*a1+x4*a2*OHM)-kr1*e1-ko1*de1-dt(1))/b1;
    vt3(k)=(-(x2*x6*a3+x2*a4*OHM)-kr2*e2-ko2*de2-dt(2))/b2;
    vt4(k)=(-(x2*x4*a5)-kr3*e3-ko3*de3-dt(3))/b3;
    
    
    U=[vt1(k);vt2(k);vt3(k);vt4(k)];

    tSpan = [0 T];
	[tt,Qq] = ode45(@(t,x) plant(t,x,U,dt),tSpan,Qk);
    Qk = Qq(length(Qq),:);
   	[tt1,Zq] = ode45(@(t,x) obv(t,x,U,Qk,ot(:,k),e(:,k),de(:,k)),tSpan,Zk);
    Zk = Zq(length(Zq),:);
                                
end

figure(1);
plot(time,phi,'k',time,phid,'r--');
xlabel('time(s)');ylabel('phi');
legend('φq','φd');
grid on;

figure(2);
plot(time,th,'k',time,thd,'r--');
xlabel('time(s)');ylabel('th');
legend('θq','θd');
grid on;
 
figure(3);
plot(time,fi,'k',time,fid,'r--');
xlabel('time(s)');ylabel('fi');
legend('ψq','ψd');
grid on;
 
figure(4);
plot(time,h,'k',time,hd,'r--');
xlabel('time(s)');ylabel('h');
legend('hq','hd');
grid on;

figure(5);
plot(time,vt1,'k');
xlabel('time(s)');ylabel('U1');
grid on;

figure(6);
plot(time,vt2,'k');
xlabel('time(s)');ylabel('U2');
grid on;

figure(7);
plot(time,vt3,'k');
xlabel('time(s)');ylabel('U3');
grid on;

figure(8);
plot(time,vt4,'k');
xlabel('time(s)');ylabel('U4');
grid on;